//
// Created by bobin on 17-11-29.
//

#include "opencv2/core/core.hpp"
#include "updator.h"
#include <string>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <highgui.h>
#include <cvaux.h>
#include "VPCluster.h"
#include "VPSample.h"
#include "lsd/lsd.h"

std::string img_file = "../data/orgin.png";

void vec_cross(float a1, float b1, float c1,
               float a2, float b2, float c2,
               float &a3, float &b3, float &c3) {
    a3 = b1 * c2 - c1 * b2;
    b3 = -(a1 * c2 - c1 * a2);
    c3 = a1 * b2 - b1 * a2;
}

void vec_norm(float &a, float &b, float &c) {
    float len = sqrt(a * a + b * b + c * c);
    a /= len;
    b /= len;
    c /= len;
}

int main(int argc, char **arrgv) {
    std::vector<std::vector<float> *> pts;
    cv::Mat color, gray;
    color = cv::imread(img_file, cv::IMREAD_COLOR);
    cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
    gray.assignTo(gray, CV_64F);
    double *out;
    int n;
    const int ndim = 7;
    out = lsd(&n, (double *) (gray.data), gray.cols, gray.rows);
    for (int i = 0; i < n; ++i) {
        double x0, y0, x1, y1;
        x0 = out[ndim * i];
        y0 = out[ndim * i + 1];
        x1 = out[ndim * i + 2];
        y1 = out[ndim * i + 3];
//        std::cout << out[ndim * i + 0] << '\t' <<
//              out[ndim * i + 1] << '\t' <<
//              out[ndim * i + 2] << '\t' <<
//              out[ndim * i + 3] << '\t' << std::endl;

        double dx = x0 - x1, dy = y0 - y1;
        if (dx * dx + dy * dy < 400)
            continue;

        std::vector<float> *p = new std::vector<float>(4);
        pts.push_back(p);
        (*p)[0] = (float) x0;
        (*p)[1] = (float) y0;
        (*p)[2] = (float) x1;
        (*p)[3] = (float) y1;
    }

    std::vector<unsigned int> Lables;
    std::vector<unsigned int> LableCount;
    std::vector<std::vector<cv::Vec3f>> vpModel;

    //2. VP cluster
    std::vector<std::vector<float> *> *mModels;
    mModels = VPSample::run(&pts, 5000, 2, 0, 3);
    int classNum = VPCluster::run(Lables, LableCount, &pts, mModels, 2, 2);
    std::cout << "vpdetection found " << classNum << " classes!" << std::endl;

    //2.1. release other resource

    for (unsigned int i = 0; i < mModels->size(); ++i) {
        delete (*mModels)[i];
    }
    delete mModels;

    unsigned int len = (unsigned int) Lables.size();
    vpModel.resize(classNum);
    for (int j = 0; j < len; ++j) {
        assert(Lables[j] < classNum);
        float line[3];
        float xs = (*pts[j])[0];
        float ys = (*pts[j])[1];
        float xe = (*pts[j])[2];
        float ye = (*pts[j])[3];

        vec_cross(xs, ys, 1,
                  xe, ye, 1,
                  line[0], line[1], line[2]);
//        vec_norm(line[0], line[1], line[2]);
        vpModel[Lables[j]].push_back(cv::Vec3f(line[0], line[1], line[2]));
    }

    for (int k = 0; k < 2; ++k) {
        int num = vpModel[k].size();
        if (num < 5)
            continue;
        cv::Mat A(num, 3, CV_32F);
        for (int i = 0; i < num; ++i) {
            A.row(i) = cv::Mat(vpModel[k][i]);
            std::cout << vpModel[k][i] << std::endl;
        }
        cv::Mat w, u, vt;
        cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
        cv::Mat v_pt;
        v_pt = vt.row(2);
        std::cout << "class: " << k << "num: " << num << vt << std::endl;
    }


    for (unsigned int i = 0; i < len; ++i) {
//        std::cout << (*pts[i])[0] << " " << (*pts[i])[1] << " "
//                  << (*pts[i])[2] << " " << (*pts[i])[3] << " " << Lables.at(i) << std::endl;
        cv::Point stt((*pts[i])[0], (*pts[i])[1]);
        cv::Point end((*pts[i])[2], (*pts[i])[3]);
        unsigned int label = Lables[i];
        cv::Scalar mark;
        switch (label) {
            case 0:
                mark = cv::Scalar(0, 0, 255);
                break;
            case 1:
                mark = cv::Scalar(0, 255, 0);
                break;
            case 2:
                mark = cv::Scalar(255, 0, 0);
                break;
            case 3:
                mark = cv::Scalar(0, 255, 255);
                break;
            case 4:
                mark = cv::Scalar(255, 0, 255);
                break;
            default:
                mark = cv::Scalar(255, 255, 255);
                break;
        }
        cv::line(color, stt, end, mark, 1, 8, 0);
    }
    cv::imshow("test", color);
    cv::imwrite("euroc-out.png", color);
    cv::waitKey(0);

    for (unsigned int i = 0; i < pts.size(); i++)
        delete pts[i];


}